//
// Created by lijia on 2021/4/25.
//

#include "system.h"
#include "joystick.h"
#include "display.h"
#include "blinker.h"
#include "gps.h"
#include "solver.h"
#include "servo.h"
#include "mpu.h"
#include "qfplib-m3.h"
#include "util.h"
#include "printf.h"
#include "usbd_cdc_if.h"

static uint8_t moduleStatus = 0, pageUpdateCounter = 0, joystickMoved = 0;
static uint32_t lastDisplayUpdateTime = 0, lastJoystickActTime = 0;
static int32_t gpsTargetBearing = 0;
static uint32_t distance = 0;
static uint8_t fnSwitch = 0, systemMode = 0;
static Direction manualDirection = {
		.yaw = 0,
		.pitch = 0
};
static char outBuf[32] = {0};

extern DisplayInfo *info;
extern GpsData *gpsData;
extern JoystickState *joystickState;

void system_tick() {
	uint32_t now = HAL_GetTick();
	blinker_tick();

	switch (fnSwitch) {
		case 0: // solver tick
			mpu_tick();
			solver_tick();
			break;
		case 1: // joystick act
			joystick_tick();
			info->joystickDirection = joystickState->direction;
			if (joystickState->direction != 0 && now - lastJoystickActTime > 1) {
				joystickMoved = 1;
				if (joystickState->direction & JOYSTICK_DIR_NORTH) {
					manualDirection.pitch -= 1;
				}
				if (joystickState->direction & JOYSTICK_DIR_SOUTH) {
					manualDirection.pitch += 1;
				}
				if (joystickState->direction & JOYSTICK_DIR_WEST) {
					manualDirection.yaw -= 1;
				}
				if (joystickState->direction & JOYSTICK_DIR_EAST) {
					manualDirection.yaw += 1;
				}

				lastJoystickActTime = now;
			}
			break;
		case 2: // display update, 2Hz
			if (gpsData != NULL) {
				info->gpsState = gpsData->fixState;
				info->localAlt = gpsData->elevation;
				info->localLng = gpsData->lng;
				info->localLat = gpsData->lat;
				info->satCount = gpsData->satCount;
			}
			if (now - lastDisplayUpdateTime > 500) {
				for (uint8_t i = 0; i < 3; i++) {
					// ?
				}
				display_repaint();
				lastDisplayUpdateTime = now;
				if (systemMode != 2) {
					if (++pageUpdateCounter > 20) {
						if (!info->page) {
							info->page = 1;
						} else {
							info->page = 0;
						}
						pageUpdateCounter = 0;
					}
				} else {
					info->page = 2;
				}
			}
			break;
		default:
			fnSwitch = 0;
			break;
	}

	if (++fnSwitch > 2) {
		fnSwitch = 0;
	}

	if (manualDirection.pitch < 0) {
		manualDirection.pitch = 0;
	}
	if (manualDirection.pitch > 1800) {
		if (manualDirection.pitch > 3600) {
			manualDirection.pitch = 0;
		} else {
			manualDirection.pitch = 1800;
		}
	}

	if (manualDirection.yaw < 0) {
		manualDirection.yaw = 0;
	}
	if (manualDirection.yaw > 1800) {
		if (manualDirection.yaw > 3600) {
			manualDirection.yaw = 0;
		} else {
			manualDirection.yaw = 1800;
		}
	}

	// servo action
	if (systemMode == 1) {
		// auto mode
		gps_angle(&gpsData->lat, &gpsData->lng, &info->remoteLat, &info->remoteLng, &gpsTargetBearing);
		solver_set_target_yaw(gpsTargetBearing / 10);
		gps_distance_cm(&info->localLat, &info->localLng, &info->remoteLat, &info->remoteLng, &distance);
		int32_t altitudeDifference = info->remoteAlt - info->localAlt;
		float basicPitch = rad2deg(qfp_fatan2(altitudeDifference, distance));
		solver_set_target_pitch(s3dot1(basicPitch));
	} else {
		// manual mode + calibration mode
		/*
		if (joystickMoved) {
			info->expectedPitch = manualDirection.pitch;
			info->expectedYaw = manualDirection.yaw;
			solver_set_target_pitch(manualDirection.pitch);
			solver_set_target_yaw(manualDirection.yaw);
		}
		 */
		const MpuCalibrationData *calib = mpu_calibration_get();
		for (uint8_t i = 0; i < 3; i++) {
			info->magLow[i] = calib->magneticLowerBound[i];
			info->magHigh[i] = calib->magneticUpperBound[i];
			info->magRaw[i] = mpu_get()->magnetic[i];
			info->magCorrected[i] = solver_get_corrected_mpu_data()->magnetic[i];
		}
	}

	const Direction *dir = solver_solve();
	servo_set_pitch_deg(dir->pitch);
	servo_set_yaw_deg(dir->yaw);
}

void system_mark_module_ready(uint8_t mask) {
	moduleStatus |= (1u << mask);
}

uint8_t system_is_module_ready(uint8_t mask) {
	return moduleStatus & (1u << mask);
}

uint8_t system_get_module_status() {
	return moduleStatus;
}

void system_clear_module_status() {
	moduleStatus = 0;
}

void system_set_mode(uint8_t x) {
	info->mode = x;
	systemMode = x;
}
